#include "lidar_det.hpp"

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
}

void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    if(use_lio)
    {
        current_point.z= current_msg->pose.pose.position.z;
    }
    else
    {
        current_point.z= current_point_z;
    }
    current_angle.x= current_msg->pose.pose.orientation.x;
    current_angle.y= current_msg->pose.pose.orientation.y;
    current_angle.z= current_msg->pose.pose.orientation.z;
    current_angle.w= current_msg->pose.pose.orientation.w;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
                                        current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);
    local_odom_flag = true;
}

// 回调函数，处理PointCloud2消息  
void standard_cloud_Callback(const sensor_msgs::PointCloud2ConstPtr& msg) {  
    std::string las_file = "/home/nvidia/gucang_ws/point_cloud/output.las";
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);  
    pcl::fromROSMsg(*msg, *cloud);  
    // 准备LAS文件的写入
    liblas::Header header;
    header.SetDataFormatId(liblas::ePointFormat1);
    header.SetPointRecordsCount(cloud->points.size());
    header.SetScale(0.01, 0.01, 0.01); // 设置点云坐标缩放比例
    header.SetOffset(0, 0, 0);         // 设置点云坐标偏移量

    std::ofstream ofs(las_file, std::ios::out | std::ios::binary);
    if (!ofs.is_open()) {
        std::cout << "LAS文件保存失败" << std::endl;
        return;
    }

    liblas::Writer writer(ofs, header);
    for (const auto &point : cloud->points) {
        liblas::Point lasPoint(&header);
        lasPoint.SetCoordinates(point.x, point.y, point.z);
        writer.WritePoint(lasPoint);
    }

    writer.WriteHeader(); // 写入文件头
    ofs.close();

}  




void livox_cloud_Callback(const lidar_detect::CustomMsgConstPtr& livox_msg_in)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_in(new pcl::PointCloud<pcl::PointXYZI>);
    auto& livox_msg = livox_msg_in;
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
        pcl::PointXYZI pt;
        pt.x = livox_msg->points[i].x;
        pt.y = livox_msg->points[i].y;
        pt.z = livox_msg->points[i].z;
        float s = livox_msg->points[i].offset_time / (float)time_end;
        pt.intensity = livox_msg->points[i].reflectivity;//livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
        // pt.curvature = s*0.1;
        pointcloud_in->push_back(pt);
    }
    if (pointcloud_in->points.size() == 0)
    {   
        std::cout << "no cloud points" << std::endl;
        return;
    }  

    Eigen::Matrix3d Rpg = current_angle_Q.toRotationMatrix();
    pcl::PointXYZI pt2;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ned(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ned_filter(new pcl::PointCloud<pcl::PointXYZI>);
    for (size_t i = 0; i < pointcloud_in->points.size(); ++i) 
    {
        double dist_body = sqrt(pointcloud_in->points[i].x*pointcloud_in->points[i].x+pointcloud_in->points[i].y*pointcloud_in->points[i].y+pointcloud_in->points[i].z*pointcloud_in->points[i].z);
        double dist_horizon = sqrt(pointcloud_in->points[i].x*pointcloud_in->points[i].x+pointcloud_in->points[i].y*pointcloud_in->points[i].y);
        
        if(abs(dist_body)<0.5 || abs(dist_body)>20.0)
        {
            continue;
        }
        //雷达下装，需要将y和z翻转
        Eigen::Vector3d PointE(pointcloud_in->points[i].x,-pointcloud_in->points[i].y,-pointcloud_in->points[i].z);
        PointE =  Rpg*PointE;
        //final_point = Rotation*origin_point + Transform
        pt2.x = PointE(0) + current_point.x;
        pt2.y = PointE(1) + current_point.y;
        pt2.z = PointE(2) + current_point.z;

        if(pt2.z<1.0)
        {
            continue;
        }
        //过滤与飞机高度差距过大的点云，目前暂定向上2米 向下2米
        if((pt2.z-current_point.z)>2.0||(pt2.z-current_point.z)<-2.0)
        {
            continue;
        }
        cloud_ned->points.push_back(pt2);
        // std::cout<<"--->lidar_cloud->points[i].intensity: "<<lidar_cloud->points[i].intensity<<std::endl;
        if(pointcloud_in->points[i].intensity>filter_intensity)
        {
            cloud_ned_filter->points.push_back(pt2);
        }
    }

    //由于点云稀疏，可能需要叠加几帧的点云，这会导致延迟！
    last_last_global_cloud_ned.swap(last_global_cloud_ned);
    last_global_cloud_ned.swap(global_cloud_ned);
    global_cloud_ned.swap(*cloud_ned_filter);
    pcl::PointCloud<pcl::PointXYZI> lidar_fusion1 = last_global_cloud_ned + global_cloud_ned;
    pcl::PointCloud<pcl::PointXYZI> lidar_fusion2 = lidar_fusion1+last_last_global_cloud_ned;

    global_cloud = lidar_fusion2.makeShared();

    //只用当前帧
    // global_cloud = cloud_ned_filter;

    //publish cloud
    sensor_msgs::PointCloud2 t_rosCloud1;
    pcl::toROSMsg(*cloud_ned,t_rosCloud1);
    t_rosCloud1.header.frame_id = "map";
    point_cloud_pub1.publish(t_rosCloud1);

    sensor_msgs::PointCloud2 t_rosCloud2;
    pcl::toROSMsg(*cloud_ned_filter,t_rosCloud2);
    t_rosCloud2.header.frame_id = "map";
    point_cloud_pub2.publish(t_rosCloud2);

}

void cluster_timer_callback(const ros::TimerEvent& /*event*/)
{
    if (global_cloud->points.size() == 0)
    {   
        // std::cout << "no global_cloud points" << std::endl;
        return;
    } 
    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
    tree->setInputCloud(global_cloud);
    std::vector<pcl::PointCloud<pcl::PointXYZI>> clusters;//the cluster finnaly
    std::vector<pcl::PointIndices> clusterIndices;// create indices class
    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec; //euclidean cluster class

    //cluster params
    ec.setClusterTolerance(ClusterTolerance);
    ec.setMinClusterSize(MinClusterSize);
    ec.setMaxClusterSize(MaxClusterSize);
    ec.setSearchMethod(tree);//use kd tree algorithm fo searching
    ec.setInputCloud(global_cloud); // feed point cloud
    ec.extract(clusterIndices); 

    for (pcl::PointIndices getIndices : clusterIndices)
    {
        pcl::PointCloud<pcl::PointXYZI> cloudCluster;
        // For each point indice in each cluster
        for (int index : getIndices.indices) 
        {
            cloudCluster.push_back(global_cloud->points[index]);
        }
        cloudCluster.width = cloudCluster.size();
        cloudCluster.height = 1;
        cloudCluster.is_dense = true;
        clusters.push_back(cloudCluster);
    }
    //reset cluster id
    clusterId = 0;
    std::deque<Box> box_buffer;
    for (pcl::PointCloud<pcl::PointXYZI> cluster : clusters)
    {
        pcl::PointXYZI minPoint, maxPoint;
        pcl::getMinMax3D(cluster, minPoint, maxPoint); //get x,y,z min and max
        //judge box size 
        double box_size = sqrt((maxPoint.x-minPoint.x)*(maxPoint.x-minPoint.x)
                                +(maxPoint.y-minPoint.y)*(maxPoint.y-minPoint.y)
                                +(maxPoint.z-minPoint.z)*(maxPoint.z-minPoint.z));
        if(box_size>2.0)
        {
            continue;
        }
        Box box;
        box.x_min = minPoint.x-0.2;
        box.y_min = minPoint.y-0.2;
        box.z_min = minPoint.z-0.2;
        box.x_max = maxPoint.x+0.2;
        box.y_max = maxPoint.y+0.2;
        box.z_max = maxPoint.z+0.2;
        box.id = clusterId;
        box.time = ros::Time::now().toSec();
        box_buffer.push_back(box);
        clusterId++;
    }

    // target_box = box_buffer;
    target_box = sort_box(box_buffer);

    for(int m=0;m<target_box.size();m++)
    {
        if(!target_box[m].empty)
        {
            int idnumber = target_box[m].id;
            draw_box(target_box[m], idnumber, 0.2, 0.0, 0.5, 0.0);
            draw_name(target_box[m], "uav: "+std::to_string(idnumber), idnumber);
            geometry_msgs::PoseStamped targetpoint;
            targetpoint.header.seq = m;
            targetpoint.pose.position.x = (target_box[m].x_min+target_box[m].x_max)/2.0;
            targetpoint.pose.position.y = (target_box[m].y_min+target_box[m].y_max)/2.0;
            targetpoint.pose.position.z = (target_box[m].z_min+target_box[m].z_max)/2.0;
            target_pub.publish(targetpoint);
        }
        
    }
        
}



void huowu_callback(const swarm_msgs::BoundingBoxes &msg)
{
    deal_detection_to_3dpose(msg, "huowu", 1.2);
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "lidar_detect");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();
    //read parameters from launch file
    nh.param<bool>("use_sitl",use_sitl,false);
    nh.param<double>("ClusterTolerance",ClusterTolerance,0.3);
    nh.param<double>("MinClusterSize",MinClusterSize,3);
    nh.param<double>("MaxClusterSize",MaxClusterSize,100);
    nh.param<double>("imu_time_offset",imu_time_offset,0.175);
    nh.param<int>("image_width",image_width,1024);
    nh.param<int>("image_height",image_height,768);
    nh.param<double>("filter_intensity",filter_intensity,0);
    nh.param<bool>("use_lio",use_lio,false);
    //Subscriber
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    local_odom_sub = nh.subscribe("/mavros/local_position/odom",10,local_odom_callback);
    state_sub = nh.subscribe("/mavros/state", 10, state_cb);
    if(1)
    {   
        standard_cloud_sub = nh.subscribe("/Laser_map",1,standard_cloud_Callback);

    }
    else
    {
        livox_cloud_sub = nh.subscribe("/livox/lidar",1,livox_cloud_Callback);
    }
    huowu_yolo_sub = nh.subscribe("/bian/huowu",10,huowu_callback);

    //Publisher
    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    target_pub = nh.advertise<geometry_msgs::PoseStamped>("/zhou/vehicle_position",1);
    huowu_pub = nh.advertise<geometry_msgs::PoseStamped>("/zhou/huowu_position",1);
    point_cloud_pub1 = nh.advertise<sensor_msgs::PointCloud2>("/zhou/pointcloud1",10);
    point_cloud_pub2 = nh.advertise<sensor_msgs::PointCloud2>("/zhou/pointcloud2",10);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_id_marker", 1);

    //Timer
    cluster_timer = nh.createTimer(ros::Duration(0.1), cluster_timer_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

void deal_detection_to_3dpose(swarm_msgs::BoundingBoxes msg, std::string class_name, double real_size)
{
    Eigen::Vector3d center_point;
    if(msg.bounding_boxes.size()>0)
    {
        int clusterId_draw = 0;
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
            clusterId_draw++;

            double newx1 = (image_width/2)-(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 ; 
            double newy1 = (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0-image_height/2;
            double center_angle = atan2(newy1, newx1);  //left rotation is + ,diverse it
            // std::cout<<"----center_angle: "<<center_angle*rad2deg<<std::endl;

            double dist_to_camera_center = sqrt(newx1*newx1+newy1*newy1);
            
            //nihe
            double pitch_deg = unionsys_core->fisheye_model_dist2pitch(dist_to_camera_center);
            double pitch_to_camera = pitch_deg*deg2rad;
            //calc box size
            double yolo_box_size = sqrt((msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)*(msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)
                                        +(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin)*(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin));
            // std::cout<<"----dist_to_camera_center: "<<dist_to_camera_center<<","<<yolo_box_size<<std::endl;

            //nihe
            double virtual_focus = unionsys_core->fisheye_model_focus(pitch_deg);
            double real_dist = (real_size*virtual_focus)/yolo_box_size;
            
            // std::cout<<"+++++>test: "<<pitch_deg<<", "<<real_dist<<std::endl;
            Eigen::Vector3d vector_ray;
            vector_ray(0)= real_dist*cos(pitch_to_camera)/sqrt(1+tan(center_angle)*tan(center_angle));
            vector_ray(1) = real_dist*cos(pitch_to_camera)*tan(center_angle)/sqrt(1+tan(center_angle)*tan(center_angle));
            //camera down need inverse the z
            vector_ray(2) = -real_dist*sin(pitch_to_camera);
            if(abs(center_angle)>90.0*deg2rad)
            {
                vector_ray(0) = -vector_ray(0);
                vector_ray(1) = -vector_ray(1);
            }
            // std::cout<<"--->camera raw pos : "<<vector_ray(0)<<","<<vector_ray(1)<<","<<vector_ray(2)<<std::endl;
            Eigen::Matrix3d Rpg_now = current_angle_Q.toRotationMatrix();

            Eigen::Vector3d vector_trans = Rpg_now*vector_ray;

            vector_trans(0) += current_point.x;
            vector_trans(1) += current_point.y;
            vector_trans(2) += current_point.z;
            // std::cout<<"--->camera predict pos : "<<vector_trans(0)<<","<<vector_trans(1)<<","<<vector_trans(2)<<std::endl;

            //pub huowu position
            geometry_msgs::PoseStamped huowutpoint;
            huowutpoint.header.seq = 1;
            huowutpoint.pose.position.x = vector_trans(0);
            huowutpoint.pose.position.y = vector_trans(1);
            huowutpoint.pose.position.z = vector_trans(2);
            huowu_pub.publish(huowutpoint);

            Box min_dist_box;
            min_dist_box.x_min = vector_trans(0)-real_size/2.0;
            min_dist_box.y_min = vector_trans(1)-real_size/2.0;
            min_dist_box.z_min = vector_trans(2)-real_size/2.0;
            min_dist_box.x_max = vector_trans(0)+real_size/2.0;
            min_dist_box.y_max = vector_trans(1)+real_size/2.0;
            min_dist_box.z_max = vector_trans(2)+real_size/2.0;
            min_dist_box.id = 0;
            
            //draw box on rviz
            draw_box(min_dist_box, clusterId_draw+100, 0.2, 0.5, 0.0, 0.5);
            draw_name(min_dist_box, class_name, clusterId_draw+100);
            
        }
    }
}



void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.1);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw

    
}

void draw_name(Box temp_box,std::string name, int id)
{
    geometry_msgs::PoseStamped box_center;
    box_center.pose.position.x = (temp_box.x_min+temp_box.x_max)/2.0;
    box_center.pose.position.y = (temp_box.y_min+temp_box.y_max)/2.0;
    box_center.pose.position.z = (temp_box.z_min+temp_box.z_max)/2.0;

    //draw target name on box
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "basic_shapes";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(0.1);
    marker_id.scale.z = 0.5;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 0;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  box_center.pose.position.x;
    name_pose.position.y =  box_center.pose.position.y;
    name_pose.position.z =  temp_box.z_max+0.5;
    marker_id.text= name;
    marker_id.pose= name_pose;
    marker_id_pub.publish(marker_id);
}

std::deque<Box> sort_box(std::deque<Box> raw_box)
{
    
    std::deque<Box> box_sorted;
    for(int i=0;i<raw_box.size();i++)
    {
        geometry_msgs::Vector3 raw_box_center;
        raw_box_center.x = (raw_box[i].x_min+raw_box[i].x_max)/2.0;
        raw_box_center.y = (raw_box[i].y_min+raw_box[i].y_max)/2.0;
        raw_box_center.z = (raw_box[i].z_min+raw_box[i].z_max)/2.0;
        double max_dist = 3.0;
        int min_dist_number = -1;
        for(int j=0;j<box_list.size();j++)
        {
            if(box_list[j].empty==true)
            {
                continue;
            }
            geometry_msgs::Vector3 box_list_center;
            box_list_center.x = (box_list[j].x_min+box_list[j].x_max)/2.0;
            box_list_center.y = (box_list[j].y_min+box_list[j].y_max)/2.0;
            box_list_center.z = (box_list[j].z_min+box_list[j].z_max)/2.0;
            
            double dist = sqrt((raw_box_center.x-box_list_center.x)*(raw_box_center.x-box_list_center.x)+
                            (raw_box_center.y-box_list_center.y)*(raw_box_center.y-box_list_center.y)+
                            (raw_box_center.z-box_list_center.z)*(raw_box_center.z-box_list_center.z));
            if(dist<max_dist)
            {
                max_dist = dist;
                min_dist_number = j;
            } 
        }  
        if(min_dist_number<0)
        {
            for(int m=0;m<box_list.size();m++)
            {
                if(box_list[m].empty==true)
                {
                    box_list[m].x_min = raw_box[i].x_min;
                    box_list[m].y_min = raw_box[i].y_min;
                    box_list[m].z_min = raw_box[i].z_min;
                    box_list[m].x_max = raw_box[i].x_max;
                    box_list[m].y_max = raw_box[i].y_max;
                    box_list[m].z_max = raw_box[i].z_max;
                    box_list[m].id = m;
                    box_list[m].time = ros::Time::now().toSec();
                    box_list[m].empty = false;
                    break;
                }
            }

        }
        else
        {
            box_list[min_dist_number].x_min = raw_box[i].x_min;
            box_list[min_dist_number].y_min = raw_box[i].y_min;
            box_list[min_dist_number].z_min = raw_box[i].z_min;
            box_list[min_dist_number].x_max = raw_box[i].x_max;
            box_list[min_dist_number].y_max = raw_box[i].y_max;
            box_list[min_dist_number].z_max = raw_box[i].z_max;
            box_list[min_dist_number].id = min_dist_number;
            box_list[min_dist_number].time = ros::Time::now().toSec();
            box_list[min_dist_number].empty = false;

        }
        
    }
    for(int k=0;k<box_list.size();k++)
    {
        if(ros::Time::now().toSec()-box_list[k].time>2.0)
        {
            box_list[k].empty = true;
        }
    }
    
            
    
    
    box_sorted = box_list;
    return box_sorted;
}